/*
 * Copyright  2017 NXP
 * All rights reserved.
 *
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <stdint.h>
#include <string.h>
#include "fsl_device_registers.h"

/*******************************************************************************
 * Definitions
 ******************************************************************************/

#define APPLICATION_BOOT_ADDRESS (void *)0x20000000

/*******************************************************************************
 * Prototypes
 ******************************************************************************/

/*******************************************************************************
 * Variables
 ******************************************************************************/
extern uint8_t application_image_start[]; /* defined in linker file. */

/*******************************************************************************
 * Code
 ******************************************************************************/

void SystemInit(void)
{
    /* setup the vector table. */
#if defined(__MCUXPRESSO)
    extern void (*const g_pfnVectors[])(void);
    SCB->VTOR = (uint32_t)&g_pfnVectors;
#else
    extern void *__Vectors;
    SCB->VTOR = (uint32_t)&__Vectors;
#endif
    SYSCON->TRACECLKDIV = 0; /* disable trace. */

    /* enable RAM banks that may be off by default at reset */
    SYSCON->AHBCLKCTRLSET[0] = SYSCON_AHBCLKCTRL0_SRAM_CTRL1_MASK
                             | SYSCON_AHBCLKCTRL0_SRAM_CTRL2_MASK
                             | SYSCON_AHBCLKCTRL0_SRAM_CTRL3_MASK
                             | SYSCON_AHBCLKCTRL0_SRAM_CTRL4_MASK;
}

typedef void(*func_0_t)(void);

void JumpToImage(void * addr)
{
    uint32_t * vectorTable = (uint32_t *)addr;
    uint32_t sp_base = vectorTable[0];
    func_0_t pc_func = (func_0_t)(vectorTable[1]);

    /* set new msp and psp. */
    __set_MSP(sp_base);
    __set_PSP(sp_base);

#if __VTOR_PRESENT == 1
    SCB->VTOR = addr;
#endif

    /* jump to application. */
    pc_func();

    /* the code should never reach here. */
    while (1)
    {}
}

/*!
 * @brief Main function
 */
int main(void)
{
    /* copy application image to sram0/1/2. */
#pragma section = "__sec_application" /* this section is defined in project option dialog. */
    uint32_t application_image_size = (uint32_t)__section_end("__sec_application") - (uint32_t)&application_image_start;

    /* copy the code for application from FLASH to the target memory. */
    memcpy(APPLICATION_BOOT_ADDRESS, (void *)application_image_start, application_image_size);

    /* jump to the application image. */
    JumpToImage(APPLICATION_BOOT_ADDRESS);
}

/* EOF. */

